
#ifndef CONSTRAINT_H_
#define CONSTRAINT_H_

#include "Core.h"
#include "Entity.h"
#include "VelocityChange.h"

/*
 * REF: http://i31www.ira.uka.de/docs/Paper_VC05.pdf
 * '..the two constraints of this joint are:
 * A(t) - B(t) <= epos
 * uA(t) - uB(t) <= evel
 */
#define EPOS                      	0.01f
#define EVEL                      	0.1f
/*
 * clamping the corrective impulses minimises instability
 */
#define MAX_DISTANCE_CORRECTION     0.2f
#define MAX_VELOCITY_CORRECTION		2.f
#define MAX_ANGULAR_CORRECTION		(Math<real>::TWO_PI)

namespace p2 {

class Anchor
{
public:
	inline Anchor(Anchor const& anchor);
	inline Anchor(Vector2<real> point);
	inline Anchor(Entity* entity, Vector2<real> point);

	inline Entity* getEntity();
	inline void setEntity(Entity* entity);
	inline Vector2<real> const& getPoint() const;
	inline void setPoint(Vector2<real> point);

private:
	Entity* _entity;
	Vector2<real> _point;
};

inline Anchor::Anchor(Anchor const& anchor)
:	_entity(anchor._entity), _point(anchor._point)
{
}
inline Anchor::Anchor(Vector2<real> point)
:	_entity(NULL), _point(point)
{
}
inline Anchor::Anchor(Entity* entity, Vector2<real> point)
:	_entity(entity), _point(point)
{
}

inline Entity* Anchor::getEntity()
{
	return _entity;
}

inline void Anchor::setEntity(Entity* entity)
{
	_entity = entity;
}

inline Vector2<real> const& Anchor::getPoint() const
{
	return _point;
}

void Anchor::setPoint(Vector2<real> point)
{
	_point = point;
}

class World;
class Constraint : public RealTimeType
{
DECLARE_RTT(Constraint, RealTimeType);

public:
	typedef std::set<Constraint* , std::less<Constraint* >, boost::fast_pool_allocator<Constraint* > > Set;

public:
	virtual ~Constraint();

	virtual void preStep(real64 dT);

	virtual bool solvePositions(real64 dT) = 0;
    virtual bool solveVelocities(real64 dT) = 0;

    virtual void computeFutureAnchorPositions(real64 dT);
    virtual void updateVelocityChange();

    inline Anchor& getAnchor0();
	inline void setAnchor0(Anchor const& anchor0);
	inline Anchor& getAnchor1();
	inline void setAnchor1(Anchor const& anchor1);
	inline void* getUserData() const;
	inline void setUserData(void* userData);
	inline Vector2<real> getWorldAnchor0() const;
	inline Vector2<real> getWorldAnchor1() const;
	inline World* getOwner();
	inline void setOwner(World* owner);

protected:
	inline Constraint(Anchor const& anchor0, Anchor const& anchor1, void* userData);

protected:
	World* _owner;
	VelocityChange _dv[2];
	Anchor _anchor0, _anchor1;
    Vector2<real> _xAth, _xBth;
    Vector2<real> _rAth, _rBth;
    Vector2<real> _uth;
    real _duth;
	real _invK;
    void* _userData;
};

inline Constraint::Constraint(Anchor const& anchor0, Anchor const& anchor1, void* userData)
:   _owner(NULL),
    _anchor0(anchor0), _anchor1(anchor1),
    _xAth(Vector2<real>::ZERO), _xBth(Vector2<real>::ZERO),
    _rAth(Vector2<real>::ZERO), _rBth(Vector2<real>::ZERO),
    _uth(Vector2<real>::ZERO),
    _duth(0.f),
    _invK(0.f),
    _userData(userData)
{
}

inline Anchor& Constraint::getAnchor0()
{
	return _anchor0;
}

inline void Constraint::setAnchor0(Anchor const& anchor0)
{
	_anchor0 = anchor0;
}

inline Anchor& Constraint::getAnchor1()
{
	return _anchor1;
}

inline void Constraint::setAnchor1(Anchor const& anchor1)
{
	_anchor1 = anchor1;
}

inline void* Constraint::getUserData() const
{
	return _userData;
}

inline void Constraint::setUserData(void* userData)
{
	_userData = userData;
}

inline Vector2<real> Constraint::getWorldAnchor0() const
{
	return _xAth + _rAth;
}

inline Vector2<real> Constraint::getWorldAnchor1() const
{
	return _xBth + _rBth;
}

inline World* Constraint::getOwner()
{
	return _owner;
}

inline void Constraint::setOwner(World* owner)
{
	_owner = owner;
}

}

#endif	//CONSTRAINT_H_
